/**
* This file is part of RESLAM.
*
* Copyright (C) 2014-2019 Schenk Fabian <schenk at icg dot tugraz dot at> (Graz University of Technology)
* For more information see <https://github.com/fabianschenk/RESLAM/>
*
* RESLAM is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RESLAM is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RESLAM. If not, see <http://www.gnu.org/licenses/>.
* 
*
*  If you use this software please cite at least one of the following publications:
*  - RESLAM: A robust edge-based SLAM system,  Schenk Fabian, Fraundorfer Friedrich, ICRA 2019
*  - Robust Edge-based Visual Odometry using Machine-Learned Edges, Schenk Fabian, Fraundorfer Friedrich, IROS 2017
*  - Combining Edge Images and Depth Maps for Robust Visual Odometry, Schenk Fabian, Fraundorfer Friedrich, BMVC 2017
*/
#pragma once
#include <mutex>
#include <condition_variable>
#include <thread>
#include <queue>

#include "../GUI/Output3DWrapper.h"
#include "ceresloopcloser.h"

namespace RESLAM
{
class FrameData;
class Tracker;
class Mapper;
namespace FernRelocLib{ class FernRelocaliser; }
class SystemSettings;

class LoopCloser
{
public:
    LoopCloser(const SystemSettings& settings, FernRelocLib::FernRelocaliser* fernRelocaliser, Mapper* globalMapperPtr, Tracker* trackerPtr);
    ~LoopCloser();
    void queueKeyFrame(FrameData* frameData);
    void loopCloserLinear(FrameData* frameData);
    void setOutputWrapper(const std::vector<IOWrap::Output3DWrapper*>& outputWrapper)
    {
        std::copy(std::begin(outputWrapper),std::end(outputWrapper),std::back_inserter(mOutputWrapper));
    }
    size_t getNumberOfFramesInFernDb() const;
    auto getNumberOfLoopClosures() const { return mEdgeConnections.size(); }
    void publishGraph();
    void resetLoopCloser(const size_t deleteTillKfId);
    void stopLoopCloser();
private:
    void addFrameConstraintToGraph(const size_t frameId1, const size_t frameId2);
    void addFrameConstraintToGraph(const CeresConstraint& c);
    std::vector<IOWrap::Output3DWrapper*> mOutputWrapper;
    CeresLoopCloser mCeresLoopCloser;
    bool optimizeLoopClosure(SE3Pose& T_l_c, const FrameData& loopCandidate, const FrameData& currFrame);
    bool checkPotentialLoop(const FrameData& candidateFrame,const FrameData& currFrame);
    void loopCloserThread();
    std::thread lcThread;
    const SystemSettings& mSystemSettings;
    std::condition_variable mLCQueueCV;
    mutable std::mutex mLCQueueMutex;
    std::queue<FrameData*> mLCQueue;
    bool mFlagLCIsRunning;// = true;
    mutable std::mutex mFlagMutex;

    //Fern relocaliser
    FernRelocLib::FernRelocaliser* mFernRelocaliser; ///< Pointer to FernRelocaliser
    Mapper* mGlobalMapperPtr;  ///< Pointer to global mapper
    Tracker* mTrackerPtr; ///< Tracker that used for loop estimation and relocalisation

    size_t nOfKeyFramesInDB{0};

    bool isLoopCloserRunning() const
    {
        std::lock_guard<std::mutex> l(mFlagMutex);
        return mFlagLCIsRunning;
    }

    void setFlagLoopCloserRunning(bool flag)
    {
        std::lock_guard<std::mutex> l(mFlagMutex);
        mFlagLCIsRunning = flag;
    }
    bool assessLoopQuality(const FrameData& frameData, const KeyframePoseVector& keyFrames) const;
    void visualizeAssessLoopQuality(const FrameData& frameData, const KeyframePoseVector& keyFrames, std::string title) const;
    RelPoseConstraints mEdgeConnections;
    ConnectivityMap mEdgeConnectivityGraph; ///< connectivity graph generated by the edges
    bool optimizeLoopClosureCeres(CeresPoseVector& ceresVectorPoses, const RelPoseConstraints& ceresConstraints);
};
}